#include <ros/ros.h>
#include <visualization_msgs/InteractiveMarkerFeedback.h>
#include <interactive_markers/interactive_marker_server.h>
#include <sstream>  // 包含 stringstream

// https://github.com/ros-visualization/visualization_tutorials/blob/indigo-devel/interactive_marker_tutorials/src/basic_controls.cpp
// https://wiki.ros.org/rviz

// 回调函数，用于处理用户的互动反馈
void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
{
    // 打印每次交互事件的标记名称和位置
    ROS_INFO_STREAM(feedback->marker_name << " is now at "
        << feedback->pose.position.x << ", " << feedback->pose.position.y
        << ", " << feedback->pose.position.z);

    // 根据不同的事件类型进行处理
    switch (feedback->event_type)
    {
        case visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK:
            ROS_INFO("Button clicked");
            break;
        
        case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
            // 在鼠标释放时处理数据
            ROS_INFO_STREAM("Mouse released at position: "
                    << feedback->pose.position.x << ", "
                    << feedback->pose.position.y << ", "
                    << feedback->pose.position.z);
            // 你可以在这里添加其他逻辑，处理鼠标释放时的特定行为
            break;

        case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
            ROS_INFO("Marker position updated (dragging or moving)");
            break;

        default:
            break;
    }
}

int main(int argc, char** argv)
{
  // 初始化ROS节点
  ros::init(argc, argv, "multiple_markers");
  ros::NodeHandle n;

  // 创建一个交互式标记服务器
  interactive_markers::InteractiveMarkerServer server("multiple_markers");

  // 定义要创建的方块数量
  const int num_cubes = 3;

  // 为每个方块创建交互式标记
  for (int i = 0; i < num_cubes; ++i) {
    // 创建一个交互式标记
    visualization_msgs::InteractiveMarker int_marker;
    int_marker.header.frame_id = "base_link";
    std::stringstream ss;
    ss << "cube_" << i;
    int_marker.name = ss.str();
    int_marker.description = "Simple 3-DOF Control";

    // 设置方块的位置
    int_marker.pose.position.x = i * 1.0;  // 沿X轴放置方块
    int_marker.pose.orientation.w = 1.0;

    // 创建一个灰色立方体作为可视化元素
    visualization_msgs::Marker box_marker;
    box_marker.type = visualization_msgs::Marker::CUBE;
    box_marker.scale.x = 0.45;
    box_marker.scale.y = 0.45;
    box_marker.scale.z = 0.45;
    box_marker.color.r = 0.5;
    box_marker.color.g = 0.5;
    box_marker.color.b = 0.5;
    box_marker.color.a = 1.0;

    visualization_msgs::InteractiveMarkerControl control;
    control.orientation.w = 1;
    control.orientation.x = 1;
    control.orientation.y = 0;
    control.orientation.z = 0;
    // control.name = "rotate_x";
    // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
    // int_marker.controls.push_back(control);
    control.name = "move_x";
    control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
    int_marker.controls.push_back(control);

    control.orientation.w = 1;
    control.orientation.x = 0;
    control.orientation.y = 0;
    control.orientation.z = 1;
    // control.name = "rotate_y";
    // control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
    // int_marker.controls.push_back(control);
    control.name = "move_y";
    control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
    int_marker.controls.push_back(control);


    // control.orientation.w = 1;
    // control.orientation.x = 0;
    // control.orientation.y = 1;
    // control.orientation.z = 0;
    // control.name = "rotate_z";
    // control.interaction_mode = InteractiveMarkerControl::ROTATE_AXIS;
    // int_marker.controls.push_back(control);
    // control.name = "move_z";
    // control.interaction_mode = InteractiveMarkerControl::MOVE_AXIS;
    // int_marker.controls.push_back(control);

    // 将交互式标记插入服务器，并设置回调函数
    server.insert(int_marker, &processFeedback);
  }

  // 应用更改
  server.applyChanges();

  // 进入主循环
  ros::spin();

  return 0;
}
